/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "COccuMCL.h"

COccuMCL::COccuMCL(){
		
		num_of_meas = 0;
		sensor_offset = 0;
		O=NULL;
		dist = NULL;
		/*
		float measurement_variance;
		float vmeas;
				
#define VMAP 0.1					// Map uncertainty
#define VANG 0.1					// Angle uncertainty
#define VR	 0.02					// Uncertainty of the range measurement
#define FALSE_PROB 0.2				// "Probability" of the false measurement
#define VMEAS ((VMAP+VANG+VR)*(VMAP+VANG+VR)) // Whole measurement uncertainty
				
		
		*/
		prob_var = 0;
		prob_max = 0;
		measurement_variance = 4.0;
		vmeas = measurement_variance*measurement_variance;
}
COccuMCL::~COccuMCL(){
	
	if(dist) free(dist);
	if(O) delete(O);
}

/**
 * Initializes the filter - uses normal random method for calculating the initial distribution
 * Loads the map from file, reserves memory for particles, sets the initial distribution
 * @p *mapfile The mapfile name
 * @p mapscale the scale factor needed to get the map coordinates into meters
 * @p num_of_particles The number of particles in the filter
 * @p init_pose the initial pose
 * @p initVariance the variance for the initial quess
 * @p num_of_meas_in_scan number of measurements used from the scan
 * @p resolution The resolution of the occupancy grid (in meters)
 * @p lineval The value the line map is set to in grid
 */
void COccuMCL::initialize(const char *mapfile,
								float mapscale, 
								int num_of_particles, 
								mcl::pose initPose, 
								mcl::pose initVariance,
								int num_of_meas_in_scan,
								float _sensor_offset,
								float resolution,
								grid_t lineval){
										
		initializeVariables(mapfile,mapscale,num_of_meas_in_scan, _sensor_offset, resolution,lineval);
				
		filt.initializeNormalRandom(initPose, initVariance,num_of_particles);													
}
/**
 * Initializes the filter - uses normal random method for calculating the initial distribution
 * Loads the map from line file, reserves memory for particles, sets the initial distribution
 * @p *mapfile The mapfile name
 * @p num_of_particles The number of particles in the filter
 * @p init_pose the initial pose
 * @p initVariance the variance for the initial quess
 * @p num_of_meas_in_scan number of measurements used from the scan
*/
void COccuMCL::initialize(const char *mapfile,
								int num_of_particles, 
								mcl::pose initPose, 
								mcl::pose initVariance,
								int num_of_meas_in_scan,
								float _sensor_offset,
								grid_t occu_val)
{
	///< initializing with small value, will be resized when linemap is read
	O = new TOccupancyGrid(1,1,0.1,Grid::TCoord2D(0,0));
	 
	if(!O->load(mapfile)){
		fprintf(stderr,"COccuMCL::initializeVariables::Unable to load the map\n");
		exit(1);
	}
	o_val = occu_val;
	int Nx,Ny;
	O->getDim(Nx, Ny);
	dist = (float *) malloc(Nx*Ny*sizeof(float));
	fprintf(stderr,"COccuMCL::initializeVariables(): Computing distance transform\n");
	O->distanceTransform(dist, o_val);	 
		
	fprintf(stderr,"COccuMCL::initializeVariables(): num_of_meas_in_scan=%d offset=%.2f\n",
					                         num_of_meas_in_scan,_sensor_offset);
		
		/// Initialize memory for the private members
	virt.alloc(num_of_meas_in_scan);
				
	num_of_meas  =	num_of_meas_in_scan;
	sensor_offset = _sensor_offset;
	filt.initializeNormalRandom(initPose, initVariance,num_of_particles);
	fprintf(stderr,"COccuMCL::Variables initialized\n");
}

/**
 * Initializes the filter - uses uniform distribution method for calculating the initial distribution
 * Loads the map from file, reserves memory for particles, sets the initial distribution
 * The initial distribution is calculated using bounding box of the map. The @dP sets the 
 * resolution of the unform grid.  WARNING the initialization might not be 
 * real-time.
 * @p *mapfile The mapfile name
 * @p mapscale the scale factor needed to get the map coordinates into meters
 * @p dP the resolution for the uniform grid
 * @p num_of_meas_in_scan number of measurements used from the scan
 */
void COccuMCL::initializeGlobal(const char *mapfile,
															float mapscale,  
															mcl::pose dP, 
															int num_of_meas_in_scan,
															float _sensor_offset)
{
	/*
		TLine bb;
		mcl::pose start,end;
		
		initializeVariables(mapfile,mapscale,num_of_meas_in_scan, _sensor_offset);
		
		bb = map.getMapBoundingBox();
		
		fprintf(stderr,"Got BB:(%.2f,%.2f),(%.2f,%.2f) ",bb.x1,bb.y1,bb.x2,bb.y2);
		start.set(bb.x1,bb.y1,0);
		end.set(bb.x2,bb.y2,2*M_PI);
		filt.initializeUniform(start, end,dP);
	*/
	
}							

/**
 * Initialises the variables
*/
void COccuMCL::initializeVariables(const char *mapfile,
															float mapscale,  
															int num_of_meas_in_scan,
															float _sensor_offset,
															float resolution,
															grid_t lineval)
{
	///< initializing with small value, will be resized when linemap is read
	O = new TOccupancyGrid(1,1,resolution,Grid::TCoord2D(0,0)); 
	if(!O->loadLineMap(mapfile, mapscale,lineval)){
				fprintf(stderr,"COccuMCL::initializeVariables::Unable to load the map\n");
				exit(1);
		}
		o_val = lineval;
		int Nx,Ny;
		O->getDim(Nx, Ny);
		dist = (float *) malloc(Nx*Ny*sizeof(float));
		fprintf(stderr,"COccuMCL::initializeVariables(): Computing distance transform\n");
		O->distanceTransform(dist, lineval);	 
		
		fprintf(stderr,"COccuMCL::initializeVariables(): Called with num_of_meas_in_scan=%d\n",num_of_meas_in_scan);
		
		/// Initialize memory for the private members
		virt.alloc(num_of_meas_in_scan);
				
		num_of_meas  =	num_of_meas_in_scan;
		sensor_offset = _sensor_offset;
		
		fprintf(stderr,"COccuMCL::Variables initialized\n");
}
/**
 * Same as init but without mallocs
 */
void COccuMCL::reinitializeNR(mcl::pose initPose,mcl::pose initVariance){
		filt.initializeNormalRandom(initPose, initVariance,filt.NumOfParticles);	
}

/**
 * Run the MCL step
 * @p dP the relative movement between steps
 * @p variance prediction error 
 * @p measurement the scan taken from new position
 */
void COccuMCL::runMCL(mcl::pose dP,mcl::pose variance, mcl::scan measurement){
		mcl::scan mcl_meas;
		int fac;
		mcl::pose var;
		static int sinceSIR=0; 
		//float sensor_offset_tmp;
		fac = measurement.N / num_of_meas; ///< The ratio between the measurement and wanted num_of_meas
		
		mcl_meas.allocate(num_of_meas);
		mcl_meas.N=num_of_meas;
		
		///Take the measurements that will be used in estimation
		/// The number is reduced for computational & efficiency reasons i.e. MCL must not be "too accurate"
		for(int i=0;i<num_of_meas;i++){
			//fprintf(stderr,"i=%d faci=%d ",i,fac*i);
				mcl_meas.r[i] = measurement.r[fac*i];	
				mcl_meas.a[i] = measurement.a[fac*i];
			if(fac*i>=measurement.N) 
					fprintf(stderr,"runMCL:: Terrible mistake - expect crash!\n");
		}
		
		
		filt.predict(dP,variance); ///<Predict
		/////////////////////////////////////////////////////////////////////////////////
		if(sqrt(last_meas_update.x*last_meas_update.x + last_meas_update.y*last_meas_update.y)>1.0 || 
				 fabs(last_meas_update.a)>(25.0*M_PI/180.0))
		{
				last_meas_update.set(0,0,0);
				//fprintf(stderr,"Updating the measurement... ")
				calculateLikelihood(mcl_meas); ///<Calculate likelihoods
				//fprintf(stderr,".... Done\n");
		}else{
				last_meas_update = last_meas_update.integrateDifferential(dP);
				//calculateLikelihood();
		}
		/////////////////////////////////////////////////////////////////////////////////
		
		//calculateLikelihood(mcl_meas); ///<Calculate likelihoods
		
		filt.normalize(); 
		var = filt.getDistributionVariances(); 
		//float rad = sqrt(var.x*var.x + var.y*var.y + var.a*var.a);
		
		///Calculate the variance of the probabilities
		float varP=0;
		float iN = 1.0 / (float) filt.NumOfParticles;
		float maxP = -1;
		for(int i = 0; i<filt.NumOfParticles;i++){
				varP += (filt.Particles[i].p - iN)*(filt.Particles[i].p - iN);
				if(filt.Particles[i].p > maxP) maxP = filt.Particles[i].p;
		}
		prob_max = maxP;
		prob_var = varP;
		///Using now stetson harrison method
		//fprintf(stderr,"BLAA (since=%d)\n",sinceSIR);
		
		if( varP > .05 && sinceSIR>5){
				//fprintf(stderr,"Performing SIR varP=%.3f (since=%d)\n",varP,sinceSIR);
				filt.SIRUpdate();
				sinceSIR = 0;
				prob_var = 0;
				prob_max = 1/filt.NumOfParticles;
		}
		sinceSIR++;
		
		free(mcl_meas.r);
		free(mcl_meas.a);
}

/**
 * Run the MCL step in initialization mode
 * @p dP the relative movement between steps
 * @p variance prediction error 
 * @p measurement the scan taken from new position
 */
void COccuMCL::runMCLInit(mcl::pose dP,mcl::pose variance, mcl::scan measurement){
		mcl::scan mcl_meas;
		int fac;
		mcl::pose var;
		static int sinceSIR=2;
		
		fac = measurement.N / num_of_meas; ///< The ratio between the measurement and wanted num_of_meas
		
		mcl_meas.allocate(num_of_meas);
		mcl_meas.N=num_of_meas;
		
		///Take the measurements that will be used in estimation
		/// The number is reduced for computational & efficiency reasons i.e. MCL must not be "too accurate"
		for(int i=0;i<num_of_meas;i++){
			//fprintf(stderr,"i=%d faci=%d ",i,fac*i);
				mcl_meas.r[i] = measurement.r[fac*i];	
				mcl_meas.a[i] = measurement.a[fac*i];
				if(fac*i>=measurement.N) 
						fprintf(stderr,"runMCL:: Terrible mistake - expect crash!\n");
		}
		
		filt.predict(dP,variance); ///<Predict
		calculateLikelihood(mcl_meas); ///<Calculate likelihoods
		filt.normalize(); 
		var = filt.getDistributionVariances(); 

		filt.SIRUpdate(); ///< allways update!
		sinceSIR++;
		
		free(mcl_meas.r);
		free(mcl_meas.a);
}


/**
 * Calculates the likelihood of particle distribution
 * uses the CParticleFilter filt and TLineMap map
 * @param meas The current measurement
 * Uses the private members:
				float *virtualr;
				float *virtuallines;
				float *virtualscan_xy;
				float *virtualscan_xy_rel;
				int num_of_meas;
 **/
void COccuMCL::calculateLikelihood(mcl::scan meas)
{
		int i,j;
		
		float dL2,dl;
		float Lik;
		float Liks;
		int outLiers;
		
		filt.Lik = 0;
		filt.outLiers = 0;
		///Cycle through each particle
		//fprintf(stderr,"Numofmeas=%d, num particles=%d\n",num_of_meas,filt.NumOfParticles);
		
		for(i=0;i<filt.NumOfParticles;i++){	
			O->getScan(filt.Particles[i].x,
								 filt.Particles[i].y, 
								 filt.Particles[i].a+sensor_offset, 
								 num_of_meas, 
								 M_PI/num_of_meas, 
								 o_val,
								 dist, 
								 virt);
			
			outLiers = 0;
			  for(int jj=0;jj<num_of_meas;jj++){
				  if(virt.r[jj]<=0) outLiers++;
				}
				filt.outLiers+=(float) outLiers;
		
				Liks = 1;
				
				for(j=0;j<num_of_meas;j++){	
						dl = meas.r[j] - virt.r[j];		// The difference between expected measurement and real measurement
		
						//If the value was ok
						if(meas.r[j]>=0 && virt.r[j]>0 && dl>-2.0){
								//if(dl < -1.5f){
								//		Lik = 0.4f;
								//		Liks = Liks*Lik;
								//}
								if(fabs(dl)<0.2) dl=0.2;
								dL2 = dl*dl;
								Lik = (1.0- FALSE_PROB) * exp(  - (dL2/(vmeas)) );
								//fprintf(stderr,"%.2f ",Lik);
								Lik += FALSE_PROB;
								Liks = Liks*Lik;
								
						}else{
								Liks *= 1;
						}
				}
				Liks =0.99*Liks+0.01; ///< Scale the probability to 0.5...1
				filt.Lik+=Liks;
				filt.Particles[i].lik=Liks;
		}
		
		filt.outLiers = (filt.outLiers / ( num_of_meas * filt.NumOfParticles))*100;

}
void COccuMCL::getScan(mcl::scan &meas, mcl::pose &p){
	Grid::TScan out;
	out.alloc(meas.N);
	
	O->getScan(p.x,
						 p.y, 
						 p.a+sensor_offset, 
						 meas.N, 
						 M_PI/meas.N, 
						 o_val,
						 dist, 
						 out);
			
	float a=sensor_offset;
	float da=(M_PI)/meas.N;
	
	for(int i = 0;i<meas.N;i++){
		meas.r[i] = out.r[i];
		meas.a[i] = a;
		a+=da;
	}
	
  out.freeData();
}
void COccuMCL::updateDt(){
	O->distanceTransform(dist, o_val);	
}